#include "modbus_handler.h"
#include "modbus_tcp_slave.h"
#include <stdlib.h>
#include "../debug.h"
#include <pthread.h>
#include <unistd.h>
#include <sys/syscall.h>
#include "../misc.h"


#define NB_CONNECTION 5

void* modbus_tcp_slave1_thread(void* arg) {

    const ConfigData *config_data = get_config_data_instance();
    Misc_InitThread(__func__);
    create_server_connect(config_data->slave1.server_ip,config_data->slave1.port);
    return NULL;
}

void* modbus_tcp_slave2_thread(void* arg) {

    const ConfigData *config_data = get_config_data_instance();
    Misc_InitThread(__func__);
    create_server_connect(config_data->slave2.server_ip,config_data->slave2.port);
    return NULL;
}


int create_server_connect(const char* server_ip, int port) {

    modbus_t *ctx = NULL;
    modbus_mapping_t *mb_mapping;
    int server_socket = -1;
    uint8_t query[MODBUS_TCP_MAX_ADU_LENGTH];
    int master_socket;
    int rc;
    fd_set ref_rdset;
    fd_set rdset;
    fd_set ref_wtset;
    fd_set wtset;    
    /* Maximum file descriptor number */
    int fdmax;

    //上下文获取
    ctx = modbus_new_tcp(NULL, port);

    // modbus_set_debug(ctx,TRUE);

    mb_mapping = modbus_mapping_new(50, 50, 20100, 20100);
    if (mb_mapping == NULL) {
        fprintf(stderr, "Failed to allocate the mapping: %s\n", modbus_strerror(errno));
        modbus_free(ctx);
        return -1;
    }

    server_socket = modbus_tcp_listen(ctx, NB_CONNECTION);
    if (server_socket == -1) {
        fprintf(stderr, "Unable to listen TCP connection\n");
        modbus_free(ctx);
        return -1;
    }


    /* Clear the reference set of socket */
    FD_ZERO(&ref_rdset);
    /* Add the server socket */
    FD_SET(server_socket, &ref_rdset);
    /* Clear the reference set of socket */
    FD_ZERO(&ref_wtset);
    /* Add the server socket */
    FD_SET(server_socket, &ref_wtset);

    /* Keep track of the max file descriptor */
    fdmax = server_socket;

    while(1) {
        rdset = ref_rdset;
        wtset = ref_wtset;   
        if (select(fdmax + 1, &rdset, &wtset, NULL, NULL) == -1) {
            perror("Server select() failure.");
            if (server_socket != -1) {
                close(server_socket);
            }
            modbus_free(ctx);
            modbus_mapping_free(mb_mapping);
        }

        /* Run through the existing connections looking for data to be
         * read */
        for (master_socket = 0; master_socket <= fdmax; master_socket++) {

            if (!FD_ISSET(master_socket, &rdset) && !FD_ISSET(master_socket, &wtset)) {
                continue;
            }

            if (master_socket == server_socket) {
                /* A client is asking a new connection */
                socklen_t addrlen;
                struct sockaddr_in clientaddr;
                int newfd;

                /* Handle new connections */
                addrlen = sizeof(clientaddr);
                memset(&clientaddr, 0, sizeof(clientaddr));
                newfd = accept(server_socket, (struct sockaddr *) &clientaddr, &addrlen);
                if (newfd == -1) {
                    perror("Server accept() error");
                } else {
                FD_SET(newfd, &ref_rdset);
                FD_SET(newfd, &ref_wtset);

                    if (newfd > fdmax) {
                        /* Keep track of the maximum */
                        fdmax = newfd;
                    }
                    printf("New connection from %s:%d on socket %d\n",
                           inet_ntoa(clientaddr.sin_addr),
                           clientaddr.sin_port,
                           newfd);
                }
            } else {
                modbus_set_socket(ctx, master_socket);
		        modbus_set_indication_timeout(ctx, 5, 0);//add 5s timeout
                rc = modbus_receive(ctx, query);
                if (rc > 0) {
                    struct modbus_info_s modbus_info;
                    int ret = parse_modbus_message(ctx, query, rc, mb_mapping, &modbus_info);
                    char buf_port[10] = {0};
                    sprintf(buf_port, "%d", port);
                    strcpy(modbus_info.name, buf_port);
                    if(ret == 0){
                        modbus_recv_message_handler(ctx, query, rc, mb_mapping, &modbus_info);
                        modbus_reply(ctx, query, rc, mb_mapping, &modbus_info);
                    }else if(ret == 1){
                         modbus_reply(ctx, query, rc, mb_mapping, &modbus_info);
                    }

                } else if (rc == -1) {
                    /* This example server in ended on connection closing or
                     * any errors. */
                    printf("Connection closed on socket %d\n", master_socket);
                    close(master_socket);

                    /* Remove from reference set */
                    FD_CLR(master_socket, &ref_rdset);
                    FD_CLR(master_socket, &ref_wtset);
                    
                    if (master_socket == fdmax) {
                        fdmax--;
                    }
                }
            }
        }
    }

    return 0;
}


